#!/usr/bin/env python3

import rospy
from DrRobotController_six_axes_can.msg import impedance_control_joints
# import DrRobotController_six_axes as dr

def callback(data):
    import DrRobotController_six_axes as dr
    dr.impedance_control_joints(id_num=data.id_num, kp=data.kp, kd=data.kd)
    rospy.loginfo("impedance_control_joints_node")


def listener():
    rospy.init_node("impedance_control_joints", anonymous=True)
    rospy.Subscriber("impedance_control_joints", impedance_control_joints, callback)
    rospy.spin()


if __name__ == '__main__':
    listener()

